#!/usr/bin/env python3
import rospy
from std_msgs.msg import String
from turtlesim.srv import TeleportAbsolute

class SerialToTurtleSimple:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('serial_to_turtle', anonymous=True)
        
        # 小海龟名称
        self.turtle_name = rospy.get_param('~turtle_name', 'turtle1')
        
        # 偏航角变量
        self.current_yaw = 0.0
        self.last_yaw = 0.0
        
        # 订阅模拟串口数据
        self.serial_sub = rospy.Subscriber(
            '/simulated_serial_data', 
            String, 
            self.serial_data_callback
        )
        
        # 等待turtlesim服务可用
        rospy.wait_for_service('/{}/teleport_absolute'.format(self.turtle_name))
        
        # 创建服务客户端
        self.teleport_client = rospy.ServiceProxy(
            '/{}/teleport_absolute'.format(self.turtle_name), 
            TeleportAbsolute
        )
        
        rospy.loginfo("精简版serial_to_turtle节点已启动")
        
    def parse_yaw_data(self, data):
        """
        解析串口数据，提取偏航角
        数据格式: "YAW:123.45"
        """
        try:
            data = data.strip()
            
            if data.startswith('YAW:'):
                yaw_str = data[4:]  # 去掉"YAW:"前缀
                yaw_deg = float(yaw_str)
                
                # 将角度转换为弧度（turtlesim使用弧度）
                yaw_rad = yaw_deg * 3.14159 / 180.0
                return yaw_rad
            else:
                rospy.logwarn("数据格式错误，期望'YAW:数值'，收到: {}".format(data))
                return None
                
        except ValueError as e:
            rospy.logwarn("无法解析偏航角数据: {}, 错误: {}".format(data, e))
            return None
            
    def set_turtle_orientation(self, yaw_rad):
        """
        设置小海龟的朝向
        yaw_rad: 偏航角（弧度）
        """
        try:
            # 保持小海龟在中心位置，只改变朝向
            current_x = 5.0
            current_y = 5.0
            
            # 调用teleport_absolute服务
            response = self.teleport_client(current_x, current_y, yaw_rad)
            
            rospy.loginfo("设置小海龟朝向: {:.2f} 弧度 ({:.1f} 度)".format(
                yaw_rad, yaw_rad * 180 / 3.14159))
            
        except rospy.ServiceException as e:
            rospy.logerr("调用teleport服务失败: {}".format(e))
            
    def serial_data_callback(self, msg):
        """串口数据回调函数"""
        data = msg.data
        rospy.logdebug("收到数据: {}".format(data))
        
        # 解析偏航角
        yaw_rad = self.parse_yaw_data(data)
        
        if yaw_rad is not None:
            # 如果偏航角变化超过阈值，更新小海龟朝向
            if abs(yaw_rad - self.last_yaw) > 0.01:
                self.set_turtle_orientation(yaw_rad)
                self.last_yaw = yaw_rad
    
    def run(self):
        """主循环"""
        rospy.loginfo("节点运行中，等待串口数据...")
        rospy.spin()

if __name__ == '__main__':
    try:
        node = SerialToTurtleSimple()
        node.run()
    except rospy.ROSInterruptException:
        pass
